#!/usr/bin/env python
import cv2, math
import numpy as np
from photogrametry.desktop_experiment.qrLoc import QrLoc
from photogrametry.calibration.conf import Conf
import matplotlib.pyplot as plt


class Pose:
    def __init__(self):
        self.q = QrLoc()
        self.mat = Conf().mat
        self.dist = Conf().dist
    
    '''
    Python: cv.SOLVEPNP_IPPE
    Infinitesimal Plane-Based Pose Estimation [41]
    Object points must be coplanar. 适合二维码

    With solvePnp_P3p (cv::SOLVEPNP_P3P) the error is about 0.02961 pixels and the order does not matter much
    '''
    
    def solve(self, f="./img/DSC_2095.JPG"):
        image_points, model_points = self.q.image_object_points(f)
        print(image_points, model_points)
        im = cv2.imread(f)
        (success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, self.mat, self.dist,
                                                                      flags=cv2.SOLVEPNP_IPPE)
        # 需要实现从旋转向量和旋转矩阵之间的互转换。 旋转向量和旋转矩阵之间可以通过罗德里格斯公式进行转换：
        rotation_matrix, jaco = cv2.Rodrigues(rotation_vector, jacobian=1)
        print("cam mat:\n {0}".format(self.mat))
        print("cam dist:\n {0}".format(self.dist))
        print("Rotation Vector:\n {0}".format(rotation_vector))
        print("Rotation matrix:\n {0}".format(rotation_matrix))
        print("Translation Vector:\n {0}".format(translation_vector))
        # self.rotationMatrixToEulerAngles(rotation_matrix)
        return rotation_matrix, translation_vector
    
    '''
    已知 Pc=R*Po+T
    定义Pc为相机坐标系的点值，Po为世界坐标系的点值，R、T为世界坐标系和相机坐标系的相对外参。
    4.1求世界坐标中的点在相机坐标系下的坐标
    当Po=[0;0;0;]时，Pc=T即世界坐标原点（二维码的中心）在相机坐标系下的坐标就为T
    同样的道理，当我们已知一点的世界坐标Po，我们就可以求得Pc

    4.2求相机在世界坐标中的坐标
    因为相机在相机坐标中相当于Pc=[0;0;0]，故Po=-R’*T 即可解得相机在世界坐标系下的坐标
    其中R’为R的逆或者转置矩阵（R是正交矩阵，R的逆等于R的转置）
    cameraPosition = -np.matrix(rotM).T * np.matrix(tvec)
    当世界坐标固定，cam坐标求角度
    '''
    
    def cam_position(self, f="./img/DSC_2063.JPG"):
        r, t = self.solve(f)
        # r2=np.linalg.inv(r)
        cam_cor_W = np.matmul(-r.T, t)
        print("相机世界坐标,非常重要", cam_cor_W, cam_cor_W.shape)
        cam_cor_Self = np.matmul(r, cam_cor_W) + t
        print("相机相机坐标,几乎都是0", cam_cor_Self)
        
        # 相机并不可能照射到自己的点，所以相机像素坐标没有意义,此外，相机坐标系，除了光心的深度0，其他的深度不可以知道（除非两个图片），所以误以为相片中心相机坐标是（cx，cy，f)《全部用像素表示》，这个是错误的。
        uv = self.find_uv_from_obj(cam_cor_W, r, t)
        print("相机像素坐标,一个无用的坐标", uv)
        
        return cam_cor_W
    
    # 这个基本准确
    def find_uv_from_obj(self, p, r_vec, t_vec):
        (uv, jacobian) = cv2.projectPoints(p, r_vec, t_vec, self.mat, self.dist)
        return uv
    
    def show_3d(self, xs, ys, zs):
        fig = plt.figure()
        ax = fig.add_subplot(111, projection='3d')
        ax.scatter(xs, ys, zs, c='r', marker='o')
        ax.set_xlabel('Y')
        ax.set_ylabel('Z')
        ax.set_zlabel('X')
        plt.title('3D point cloud: Use pan axes button below to inspect')
        plt.show()
    
    # 旋转矩阵T和inv相等
    def isRotationMatrix(self, R):
        Rt = np.transpose(R)
        shouldBeIdentity = np.dot(Rt, R)
        I = np.identity(3, dtype=R.dtype)
        n = np.linalg.norm(I - shouldBeIdentity)
        return n < 1e-6
    
    # 计算旋转矩阵转换成欧拉角的过程，欧拉角不直观，只是反映变化过程
    def rotationMatrixToEulerAngles(self, R):
        assert (self.isRotationMatrix(R))
        sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
        singular = sy < 1e-6
        if not singular:
            roll = 180 * math.atan2(-R[2][1], R[2][2]) / math.pi
            pitch = 180 * math.asin(R[2][0]) / math.pi
            yaw = 180 * math.atan2(-R[1][0], R[0][0]) / math.pi
        else:
            roll = math.atan2(-R[1, 2], R[1, 1])
            pitch = math.atan2(-R[2, 0], sy)
            yaw = 0
        rot_params = [roll, pitch, yaw]
        print("euler===", rot_params)
        
        return rot_params


if __name__ == '__main__':
    p = Pose()
    fs = ["./img/DSC_2087.JPG", "./img/DSC_2090.JPG", ]
    p.cam_position()
